#include "MITC4PlateCalculator.h"

#include "SGFem/Integration/IntegrationFormula/Gauss2D1X1P.h"

using SG::DataStructure::Common::Real;
using SG::Algebra::Matrix;
using SG::DataStructure::FEM::ShellSecMaterialConstant;
using SG::FEMSolver::Element::Gauss2D1X1P;
using SG::FEMSolver::Element::Gauss2D2X2P;

std::size_t MITC4PlateCalculator::s_nNode = 4;

void MITC4PlateCalculator::Initialize ()
{    
    const auto& integ = m_integForm.GetPoints ();
    m_shpFunc.Compute (integ);

    const auto& integ0 = m_integForm0.GetPoints ();
    m_shpFunc0.Compute (integ0);

}

/// @brief 计算弯曲部分应变矩阵`B`
void MITC4PlateCalculator::ComputeBb (std::size_t iGauss, const Matrix& coord, _OUT Matrix& B, _OUT Real& j, bool isCenter) const
{
    Matrix DNDxi;
    if (isCenter)
    {
        DNDxi = m_shpFunc0.GetShapeFunctionDerivative(0);
    }
    else
    {
        DNDxi = m_shpFunc.GetShapeFunctionDerivative(iGauss);
    }

    auto JT = DNDxi * coord;
    j = JT(0, 0) * JT(1, 1) - JT(0, 1) * JT(1, 0);
    auto JTinv = JT.Inverse();
    auto DNDx = JTinv * DNDxi;

    B = Matrix(3, 12);
    for (std::size_t iNode = 0; iNode < s_nNode; ++iNode)
    {
        B(0, iNode*3 + 2) = -DNDx(0, iNode);
        B(1, iNode*3 + 1) =  DNDx(1, iNode);
        B(2, iNode*3 + 1) =  DNDx(0, iNode);
        B(2, iNode*3 + 2) = -DNDx(1, iNode);
    }
}

/** @brief 使用 Mix Interpolation of Tensor Component技术克服剪切锁定现象
 * 
 *  使用边中点处切应力对积分点切应变插值 [既增强假定应变（Enhanced Assumed Strain）技术]
 */
void MITC4PlateCalculator::ComputeBs (const SG::Algebra::Matrix& coord, SG::DataStructure::Common::Real j, SG::DataStructure::Common::Real xi, SG::DataStructure::Common::Real eta, _OUT Matrix& Bs)
{
    Real dx21 = coord (1, 0) - coord(0, 0);
    Real dx32 = coord (2, 0) - coord(1, 0);
    Real dx34 = coord (2, 0) - coord(3, 0);
    Real dx41 = coord (3, 0) - coord(0, 0);

    Real dy21 = coord (1, 1) - coord(0, 1);
    Real dy32 = coord (2, 1) - coord(1, 1);
    Real dy34 = coord (2, 1) - coord(3, 1);
    Real dy41 = coord (3, 1) - coord(0, 1);


    Real Ax = -coord (0, 0) + coord (1, 0) + coord (2, 0) - coord (3, 0);
	Real Ay = -coord (0, 1) + coord (1, 1) + coord (2, 1) - coord (3, 1);
	Real Bx =  coord (0, 0) - coord (1, 0) + coord (2, 0) - coord (3, 0);
	Real Cx = -coord (0, 0) - coord (1, 0) + coord (2, 0) + coord (3, 0);
	Real By =  coord (0, 1) - coord (1, 1) + coord (2, 1) - coord (3, 1);
	Real Cy = -coord (0, 1) - coord (1, 1) + coord (2, 1) + coord (3, 1);
    Real alph = atan(Ay / Ax);
    Real beta = 3.141592653589793 / 2 - atan(Cx / Cy);
	Matrix Rot(2, 2, { sin(beta), -sin(alph),
	                  -cos(beta),  cos(alph)});
    

    // r1、r2 本质上应该为一个协变基矢量
    Real r1 = (Cx + xi * Bx) * (Cx + xi * Bx) + (Cy + xi * By) * (Cy + xi * By);
    r1 = sqrt(r1)/(8 * j);
	Real r2 = (Ax + eta * Bx) * (Ax + eta * Bx) + (Ay + eta * By)* (Ay + eta * By);
	r2 = sqrt(r2)/(8 * j);

    Real tmp1 = r1*(1 - eta);
    Real tmp2 = r1*(1 + eta);
    Bs = Matrix(2, 12);
    const Real half{0.5};
    const Real quarter{0.25};
    Bs (0, 0) = -tmp1 * half;
    Bs (0, 1) = -tmp1 * dy21*quarter;
    Bs (0, 2) =  tmp1 * dx21*quarter;
    Bs (0, 3) =  tmp1 * half;
    Bs (0, 4) = -tmp1 * dy21*quarter;
    Bs (0, 5) =  tmp1 * dx21*quarter;

    Bs (0, 6) =   tmp2 * half;
    Bs (0, 7) =  -tmp2 * dy34*quarter;
    Bs (0, 8) =   tmp2 * dx34*quarter;
    Bs (0, 9) =  -tmp2 * half;
    Bs (0, 10) = -tmp2 * dy34*quarter;
    Bs (0, 11) =  tmp2 * dx34*quarter;

    tmp1 = r2*(1 - xi);
    tmp2 = r2*(1 + xi);
    Bs (1, 0) =  -tmp1 * 0.5;
    Bs (1, 1) =  -tmp1 * dy41*0.25;
    Bs (1, 2) =   tmp1 * dx41*0.25;
    Bs (1, 9) =   tmp1 * 0.5;
    Bs (1, 10) = -tmp1 * dy41*0.25;
    Bs (1, 11) =  tmp1 * dx41*0.25;

    Bs (1, 3) = -tmp2 * 0.5;
    Bs (1, 4) = -tmp2 * dy32*0.25;
    Bs (1, 5) =  tmp2 * dx32*0.25;
    Bs (1, 6) =  tmp2 * 0.5;
    Bs (1, 7) = -tmp2 * dy32*0.25;
    Bs (1, 8) =  tmp2 * dx32*0.25;

    Bs = Rot * Bs;
}


Matrix MITC4PlateCalculator::ComputeStiffness 
(
    const SG::Algebra::Matrix& coord, 
    const SG::DataStructure::Common::Real* const thickness,
    const SG::DataStructure::FEM::ShellSecMaterialConstant& SecConstant,
    const SG::Algebra::Matrix& T1, 
    const SG::Algebra::Matrix& T2, 
    const bool &transFlag
) const
{
    Matrix ke (12, 12);
    Matrix Bb;
    Matrix Bs;
    Real j;

    Matrix Cb;
    Matrix Cs;
    const auto nGauss = m_integForm.GetPointSize();
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        SecConstant.GetCb (thickness[iGauss], _OUT Cb);
        SecConstant.GetCs (thickness[iGauss], _OUT Cs);
        //若需要材料坐标转换
        if (transFlag)
        {
           Cb = T1 * Cb * T1.Transpose ();
           Cs = T2 * Cs * T2.Transpose ();
        }
        ComputeBb (iGauss, coord, _OUT Bb, _OUT j);
        auto xi = m_integForm.GetPoint(iGauss).m_x;
        auto eta = m_integForm.GetPoint(iGauss).m_y;
        ComputeBs (coord, j, xi, eta,_OUT Bs);

        const Real tmp = j * m_integForm.GetPoint(iGauss).m_weight;
        ke += Bb.Transpose() * Cb * Bb * tmp;
        ke += Bs.Transpose() * Cs * Bs * tmp;
    }
    return ke;
}